//////////////////////////////////////////ok
#include"stdafx.h"
#include "bochs.h"



void IA32_CPU::prepareSSE(void)
{
  if( cr0.ts)
    exception(IA32_NM_EXCEPTION, 0, 0);

  if( cr0.em)
    exception(IA32_UD_EXCEPTION, 0, 0);

  if(!( cr4.get_OSFXSR()))
    exception(IA32_UD_EXCEPTION, 0, 0);
}

#define IA32_MXCSR_REGISTER ( mxcsr.mxcsr)



/* ************************************ */
/* SSE: SAVE/RESTORE FPU/MMX/SSEx STATE */
/* ************************************ */

/* 0F AE Grp15 010 */
void IA32_CPU::LDMXCSR(Ia32_Instruction_c *i)
{
   prepareSSE();

  Bit32u new_mxcsr;

  read_virtual_dword(i->seg(), IA32_RMAddr(i), &new_mxcsr);
  if(new_mxcsr & ~IA32_MXCSR_MASK)
      exception(IA32_GP_EXCEPTION, 0, 0);

  IA32_MXCSR_REGISTER = new_mxcsr;

}

/* 0F AE Grp15 011 */
void IA32_CPU::STMXCSR(Ia32_Instruction_c *i)
{
   prepareSSE();

  Bit32u MxData = IA32_MXCSR_REGISTER & IA32_MXCSR_MASK;
  write_virtual_dword(i->seg(), IA32_RMAddr(i), &MxData);

}

/* 0F AE Grp15 000 */
void IA32_CPU::FXSAVE(Ia32_Instruction_c *i)
{
  unsigned index;
  Ia32PackedXmmRegister xmm;

  if( cr0.ts)
    exception(IA32_NM_EXCEPTION, 0, 0);

  if( cr0.em)
    exception(IA32_UD_EXCEPTION, 0, 0);

  xmm.xmm16u(0) =  the_i387.get_control_word();
  xmm.xmm16u(1) =  the_i387.get_status_word ();

  Bit16u twd =  the_i387.get_tag_word(), tag_byte = 0;

  if((twd & 0x0003) != 0x0003) tag_byte |= 0x01;
  if((twd & 0x000c) != 0x000c) tag_byte |= 0x02;
  if((twd & 0x0030) != 0x0030) tag_byte |= 0x04;
  if((twd & 0x00c0) != 0x00c0) tag_byte |= 0x08;
  if((twd & 0x0300) != 0x0300) tag_byte |= 0x10;
  if((twd & 0x0c00) != 0x0c00) tag_byte |= 0x20;
  if((twd & 0x3000) != 0x3000) tag_byte |= 0x40;
  if((twd & 0xc000) != 0xc000) tag_byte |= 0x80;

  xmm.xmm16u(2) = tag_byte;

  xmm.xmm16u(3) = 0;  /* still not implemented */

  xmm.xmm64u(1) = 0;  /* still not implemented */
 
  write_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i), (Bit8u *) &xmm);

  xmm.xmm64u(0) = 0;  /* still not implemented */

  xmm.xmm32u(2) = IA32_MXCSR_REGISTER;
  xmm.xmm32u(3) = IA32_MXCSR_MASK;

  write_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i) + 16, (Bit8u *) &xmm);

  for(index=0; index < 8; index++)
  {
    const floatx80 &fp = IA32_FPU_REG(index);

    xmm.xmm64u(0) = fp.fraction;
    xmm.xmm64u(1) = 0;
    xmm.xmm16u(4) = fp.exp;
    
    write_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i)+index*16+32, (Bit8u *) &xmm);
  }

  for(index=0; index < IA32_XMM_REGISTERS; index++)
  {
    write_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i)+index*16+160, (Bit8u *) &(this->xmm[index]));
  }

}

/* 0F AE Grp15 001 */
void IA32_CPU::FXRSTOR(Ia32_Instruction_c *i)
{
	Ia32PackedXmmRegister xmm;
	int index;

	if(cr0.ts)
		exception(IA32_NM_EXCEPTION, 0, 0);

	if(cr0.em)
		exception(IA32_UD_EXCEPTION, 0, 0);

	read_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i), (Bit8u *) &xmm);
	the_i387.cwd = xmm.xmm16u(0);
	the_i387.swd = xmm.xmm16u(1);
	the_i387.tos = (xmm.xmm16u(1) >> 11) & 0x07;

	Bit32u twd = 0, tag_byte = xmm.xmm16u(2);
	if(cr4.get_OSFXSR())
	{
		read_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i) + 16, (Bit8u *) &xmm);
		Bit32u new_mxcsr = xmm.xmm32u(2);
		if(new_mxcsr & ~IA32_MXCSR_MASK)
			exception(IA32_GP_EXCEPTION, 0, 0);
		IA32_MXCSR_REGISTER = new_mxcsr;
	}


	for(index=0; index < 8; index++)
	{
		read_virtual_tword(i->seg(), 
		IA32_RMAddr(i)+index*16+32, &(IA32_FPU_REG(index)));
	}


	for(index = 7;index >= 0; index--, twd <<= 2, tag_byte <<= 1)
	{
		if(tag_byte & 0x80) 
		{
			const floatx80 &fpu_reg = IA32_FPU_REG(index);
			twd |= FPU_tagof(fpu_reg);
		}
		else {
			twd |= FPU_Tag_Empty;
		}
	}

	the_i387.twd = (twd >> 2);
	if(cr4.get_OSFXSR())
	{
		/* load XMM register file */
		for(index=0; index < IA32_XMM_REGISTERS; index++)
		{
			read_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i)+index*16+160, (Bit8u *) &(this->xmm[index]));
		}
	}

}

/* *************************** */
/* SSE: MEMORY MOVE OPERATIONS */
/* *************************** */

/* All these opcodes never generate SIMD floating point exeptions */

/* MOVUPS:    0F 10 */
/* MOVUPD: 66 0F 10 */
/* MOVDQU: F3 0F 6F */
void IA32_CPU::MOVUPS_VpsWps(Ia32_Instruction_c *i)
{
   prepareSSE();

  Ia32PackedXmmRegister op;

  if (i->modC0()) 
  {
    op = IA32_READ_XMM_REG(i->rm());
  }
  else 
  {
    read_virtual_dqword(i->seg(), IA32_RMAddr(i), (Bit8u *) &op);
  }

  IA32_WRITE_XMM_REG(i->nnn(), op);
}

/* MOVUPS:    0F 11 */
/* MOVUPD: 66 0F 11 */
/* MOVDQU: F3 0F 7F */
void IA32_CPU::MOVUPS_WpsVps(Ia32_Instruction_c *i)
{
   prepareSSE();

  Ia32PackedXmmRegister op = IA32_READ_XMM_REG(i->nnn());

  if (i->modC0()) 
  {
    IA32_WRITE_XMM_REG(i->rm(), op);
  }
  else 
  {
    write_virtual_dqword(i->seg(), IA32_RMAddr(i), (Bit8u *) &op);
  }

}

/* MOVAPS:    0F 28 */
/* MOVAPD: 66 0F 28 */
/* MOVDQA: F3 0F 6F */
void IA32_CPU::MOVAPS_VpsWps(Ia32_Instruction_c *i)
{
   prepareSSE();

  Ia32PackedXmmRegister op;

  if (i->modC0()) 
  {
    op = IA32_READ_XMM_REG(i->rm());
  }
  else 
  {
    read_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i), (Bit8u *) &op);
  }

  IA32_WRITE_XMM_REG(i->nnn(), op);
}

/* MOVAPS:    0F 29 */
/* MOVAPD: 66 0F 29 */
/* MOVDQA: F3 0F 7F */
void IA32_CPU::MOVAPS_WpsVps(Ia32_Instruction_c *i)
{
   prepareSSE();

  Ia32PackedXmmRegister op = IA32_READ_XMM_REG(i->nnn());

  if (i->modC0()) 
  {
    IA32_WRITE_XMM_REG(i->rm(), op);
  }
  else 
  {
    write_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i), (Bit8u *) &op);
  }

}

/* F3 0F 10 */
void IA32_CPU::MOVSS_VssWss(Ia32_Instruction_c *i)
{
   prepareSSE();

  Ia32PackedXmmRegister op1 = IA32_READ_XMM_REG(i->nnn()), op2;
  Bit32u val32;

  if (i->modC0()) 
  {
    op2 = IA32_READ_XMM_REG(i->rm());
    op1.xmm32u(0) = op2.xmm32u(0);
  }
  else 
  {
    read_virtual_dword(i->seg(), IA32_RMAddr(i), &val32);
    op1.xmm32u(0) = val32;
    op1.xmm32u(1) = 0;
    op1.xmm64u(1) = 0;
  }

  IA32_WRITE_XMM_REG(i->nnn(), op1);

}

/* F3 0F 11 */
void IA32_CPU::MOVSS_WssVss(Ia32_Instruction_c *i)
{
   prepareSSE();

  Bit32u val32 = IA32_READ_XMM_REG_LO_DWORD(i->nnn());

  if (i->modC0()) 
  {
    IA32_WRITE_XMM_REG_LO_DWORD(i->rm(), val32);
  }
  else 
  {
    write_virtual_dword(i->seg(), IA32_RMAddr(i), &val32);
  }

}

/* F2 0F 10 */
void IA32_CPU::MOVSD_VsdWsd(Ia32_Instruction_c *i)
{
   prepareSSE();

  Ia32PackedXmmRegister op1 = IA32_READ_XMM_REG(i->nnn()), op2;
  Bit64u val64;

  if (i->modC0()) 
  {
    op2 = IA32_READ_XMM_REG(i->rm());
    op1.xmm64u(0) = op2.xmm64u(0);
  }
  else 
  {
    read_virtual_qword(i->seg(), IA32_RMAddr(i), &val64);
    op1.xmm64u(0) = val64;
    op1.xmm64u(1) = 0;
  }

  IA32_WRITE_XMM_REG(i->nnn(), op1);
}

/* F2 0F 11 */
void IA32_CPU::MOVSD_WsdVsd(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* MOVLPS:    0F 12 */
/* MOVLPD: 66 0F 12 */
void IA32_CPU::MOVLPS_VpsMq(Ia32_Instruction_c *i)
{
   prepareSSE();
  Bit64u val64;

  if (i->modC0()) /* MOVHLPS xmm1, xmm2 opcode */
  {
    val64 = IA32_READ_XMM_REG_HI_QWORD(i->rm());
  }
  else 
  {
    read_virtual_qword(i->seg(), IA32_RMAddr(i), &val64);
  }

  IA32_WRITE_XMM_REG_LO_QWORD(i->nnn(), val64);
}

/* F2 0F 12 */
void IA32_CPU::MOVDDUP_VpdWq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* F3 0F 12 */
void IA32_CPU::MOVSLDUP_VpsWps(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* F3 0F 16 */
void IA32_CPU::MOVSHDUP_VpsWps(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* MOVLPS:    0F 13 */
/* MOVLPD: 66 0F 13 */
void IA32_CPU::MOVLPS_MqVps(Ia32_Instruction_c *i)
{
   prepareSSE();

  if (i->modC0()) 
  {
    UndefinedOpcode(i);
  }
  write_virtual_qword(i->seg(), IA32_RMAddr(i), &IA32_XMM_REG_LO_QWORD(i->nnn()));
}

/* MOVHPS:    0F 16 */
/* MOVHPD: 66 0F 16 */
void IA32_CPU::MOVHPS_VpsMq(Ia32_Instruction_c *i)
{
   prepareSSE();
  Bit64u val64;

  if (i->modC0()) /* MOVLHPS xmm1, xmm2 opcode */
  {
    val64 = IA32_READ_XMM_REG_LO_QWORD(i->rm());
  }
  else 
  {
    read_virtual_qword(i->seg(), IA32_RMAddr(i), &val64);
  }

  IA32_WRITE_XMM_REG_HI_QWORD(i->nnn(), val64);

}

/* MOVHPS:    0F 17 */
/* MOVHPD: 66 0F 17 */
void IA32_CPU::MOVHPS_MqVps(Ia32_Instruction_c *i)
{
   prepareSSE();

  if (i->modC0()) 
  {
    UndefinedOpcode(i);
  }

  write_virtual_qword(i->seg(), IA32_RMAddr(i), &IA32_XMM_REG_HI_QWORD(i->nnn()));

}

/* F2 0F F0 */
void IA32_CPU::LDDQU_VdqMdq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* 66 0F F7 */
void IA32_CPU::MASKMOVDQU_VdqVRdq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* 0F 50 */
void IA32_CPU::MOVMSKPS_GdVRps(Ia32_Instruction_c *i)
{
   prepareSSE();

  Ia32PackedXmmRegister op = IA32_READ_XMM_REG(i->nnn());
  Bit32u val32 = 0;

  if(op.xmm32u(0) & 0x80000000) val32 |= 0x1;
  if(op.xmm32u(1) & 0x80000000) val32 |= 0x2;
  if(op.xmm32u(2) & 0x80000000) val32 |= 0x4;
  if(op.xmm32u(3) & 0x80000000) val32 |= 0x8;

  IA32_WRITE_32BIT_REG(i->rm(), val32);

}

/* 66 0F 50 */
void IA32_CPU::MOVMSKPD_EdVRpd(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* 66 0F 6E */
void IA32_CPU::MOVD_VdqEd(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* 66 0F 7E */
void IA32_CPU::MOVD_EdVd(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* F3 0F 7E */
void IA32_CPU::MOVQ_VqWq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* 66 0F D6 */
void IA32_CPU::MOVQ_WqVq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* F2 0F D6 */
void IA32_CPU::MOVDQ2Q_PqVRq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* F3 0F D6 */
void IA32_CPU::MOVQ2DQ_VdqQq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* ****************************** */
/* SSE: MEMORY SHUFFLE OPERATIONS */
/* ****************************** */

/* 0F C6 */
void IA32_CPU::SHUFPS_VpsWpsIb(Ia32_Instruction_c *i)
{
   prepareSSE();

  Ia32PackedXmmRegister op1 = IA32_READ_XMM_REG(i->nnn()), op2, result;
  Bit8u order = i->Ib();

  /* op2 is a register or memory reference */
  if (i->modC0()) 
  {
    op2 = IA32_READ_XMM_REG(i->rm());
  }
  else 
  {
    read_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i), (Bit8u *) &op2);
  }

  result.xmm32u(0) = op1.xmm32u((order >> 0) & 0x3);
  result.xmm32u(1) = op1.xmm32u((order >> 2) & 0x3);
  result.xmm32u(2) = op2.xmm32u((order >> 4) & 0x3);
  result.xmm32u(3) = op2.xmm32u((order >> 6) & 0x3);
  IA32_WRITE_XMM_REG(i->nnn(), result);

}

/* 66 0F C6 */
void IA32_CPU::SHUFPD_VpdWpdIb(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* 66 0F 60 */
void IA32_CPU::PUNPCKLBW_VdqWq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* 66 0F 61 */
void IA32_CPU::PUNPCKLWD_VdqWq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* UNPCKLPS:     0F 14 */
/* PUNPCKLDQ: 66 0F 62 */

void IA32_CPU::PUNPCKLDQ_VdqWq(Ia32_Instruction_c *i)
{
   prepareSSE();

  Ia32PackedXmmRegister op1 = IA32_READ_XMM_REG(i->nnn()), op2, result;

  if (i->modC0()) 
  {
    op2 = IA32_READ_XMM_REG(i->rm());
  }
  else 
  {
    read_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i), (Bit8u *) &op2);
  }

  result.xmm32u(0) = op1.xmm32u(0);
  result.xmm32u(1) = op2.xmm32u(0);
  result.xmm32u(2) = op1.xmm32u(1);
  result.xmm32u(3) = op2.xmm32u(1);
  IA32_WRITE_XMM_REG(i->nnn(), result);

}

/* 66 0F 68 */
void IA32_CPU::PUNPCKHBW_VdqWq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* 66 0F 69 */
void IA32_CPU::PUNPCKHWD_VdqWq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* UNPCKHPS:     0F 15 */
/* PUNPCKHDQ: 66 0F 6A */

void IA32_CPU::PUNPCKHDQ_VdqWq(Ia32_Instruction_c *i)
{
   prepareSSE();

  Ia32PackedXmmRegister op1 = IA32_READ_XMM_REG(i->nnn()), op2, result;

  if (i->modC0()) 
  {
    op2 = IA32_READ_XMM_REG(i->rm());
  }
  else 
  {
    read_virtual_dqword_aligned(i->seg(), IA32_RMAddr(i), (Bit8u *) &op2);
  }

  result.xmm32u(0) = op1.xmm32u(2);
  result.xmm32u(1) = op2.xmm32u(2);
  result.xmm32u(2) = op1.xmm32u(3);
  result.xmm32u(3) = op2.xmm32u(3);

  IA32_WRITE_XMM_REG(i->nnn(), result);
}

/* UNPCKLPD:   66 0F 14 */
/* PUNPCKLQDQ: 66 0F 6C */

void IA32_CPU::PUNPCKLQDQ_VdqWq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* UNPCKHPD:   66 0F 15 */
/* PUNPCKHQDQ: 66 0F 6D */

void IA32_CPU::PUNPCKHQDQ_VdqWq(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* 66 0F 70 */
void IA32_CPU::PSHUFD_VdqWdqIb(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* F2 0F 70 */
void IA32_CPU::PSHUFHW_VqWqIb(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* F3 0F 70 */
void IA32_CPU::PSHUFLW_VqWqIb(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* **************************** */
/* SSE: STORE DATA NON-TEMPORAL */
/* **************************** */

/* 0F C3 */
void IA32_CPU::MOVNTI_MdGd(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* MOVNTPS:    0F 2B */
/* MOVNTPD: 66 0F 2B */
/* MOVNTDQ: 66 0F E7 */
void IA32_CPU::MOVNTPS_MdqVps(Ia32_Instruction_c *i)
{
   prepareSSE();

  if (i->modC0()) 
  {
    UndefinedOpcode(i);
  }

  write_virtual_dqword(i->seg(), IA32_RMAddr(i), (Bit8u *)(&IA32_READ_XMM_REG(i->nnn())));

}
